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Summary 

This paper presents the forward position kine- 
matics (given the eight joint angles, how to find 
the Cartesian position and orientation of the end ef- 
fector) and forward velocity kinematics (given the 
eight joint rates, how to find the Cartesian transla- 
tional and rotational velocities of the end effector) 
for the redundant eight-degree-of-freedom Advanced 
Research Manipulator II (ARMII). 

Inverse kinematic solutions, required to control 
the manipulator end effector, are also presented. For 
a redundant manipulator, the inverse kinematic so- 
lutions are not unique because they involve solving 
for eight unknowns (joint angles for inverse position 
and joint rates for inverse velocity) in only six equa- 
tions. The approach in this paper is to specify two 
of the unknowns and solve for the remaining six un- 
knowns. Two unknowns can be specified with two 
restrictions. First, the elbow joint angle and rate 
cannot be specified. The elbow joint angle is deter- 
mined solely by the commanded position of the end 
effector. Likewise, the elbow joint rate is determined 
by the commanded Cartesian translational velocity 
of the end effector. Second, one unknown must be 
specified from the four-jointed wrist, while the sec- 
ond unknown must be specified from one of the arm 
joints (elbow joint excluded) that translate the wrist. 

The inverse position solution has eight solutions 
for each set of two specified joint angles. No alternate 
inverse position solutions arc presented for singular 
configurations. In the inverse velocity problem, with 
two specified joint rates, the solution is unique pro- 
vided that the Jacobian matrix is nonsingular. A 
discussion of singularities is based on specifying two 
joint rates and analyzing the reduced Jacobian ma- 
trix. When the reduced Jacobian matrix is singular, 
the generalized inverse can be used to move the ma- 
nipulator away from the singularity region. 

With two redundant joints, the methods of this 
paper allow considerable freedom in solving the in- 
verse kinematic problems. However, no control 
strategies are developed to move the manipulator. 
Control strategies are developed through ARMII 
hardware experience. 

A symbolic manipulation computer program was 
used with existing standard methods in robotics for 
the derivation of the equations. In addition, com- 
puter simulations were developed to verify the equa- 
tions. Examples demonstrate agreement between 
forward and inverse solutions. 


1. Introduction 

The Advanced Research Manipulator II (ARMII), 
a redundant research manipulator built by the AAI 
Corporation for NASA, is well suited for space tele- 
robotic applications and earth-based simulations of 
space telerobotic applications. The ARMII has sev- 
eral features that distinguish it from common indus- 
trial manipulators: (1) two redundant degrees of free- 
dom, (2) high payload-to-weight ratio with a 40-lb 
design payload at a 60-in. reach, (3) modular joint 
design, (4) high joint and link stiffness with graphite- 
epoxy composite link material, (5) continuous bi- 
directional end-effector roll, (6) input and output 
joint position encoders, and (7) space flight quali- 
fiable components. This paper presents kinematic 
equations that can be implemented for basic control 
of the ARMII. 

NASA Langley Research Center has two ARMIFs 
for investigation of redundant dual arm control and 
disturbance compensation for space operations. Fig- 
ure 1 is a photograph and figure 2 is a schematic 
diagram of the ARMII, a redundant serial manip- 
ulator with eight revolutc joints. For general spa- 
tial tasks, six degrees of freedom are required. The 
ARMII has two redundant joints; with this extra 
freedom, performance criteria can be satisfied in ad- 
dition to the commanded motion. The use of this 
redundancy is not presented in this paper; how- 
ever, references 1 to 8 present control methods for 
redundant manipulators. These references use ma- 
nipulator redundancy to satisfy performance crite- 
ria, such as singularity avoidance, joint limit avoid- 
ance, minimization of joint rates, minimization of 
manipulator energy, and optimization of manipula- 
tor configuration. 

The ARMII forward position, inverse position, 
forward velocity, and inverse velocity problems are 
formulated and solved in this paper. The forward 
solutions are given for all eight degrees of freedom. 
The term forward position transformation hereafter 
indicates both position and orientation. The inverse 
solutions involve six equations in eight unknowns. 
The inverse solutions in this paper require that two 
of the eight joint angles and rates are specified and 
then the remaining six joint angles and rates are 
solved. Joint angles and rates for different joints can 
be specified at each calculation step. Therefore, this 
approach is more general than one that locks two 
joints for all motion. 

The forward position transformation is presented 
after a discussion of kinematic simplification. With 
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the conventions of Craig (ref. 9), the Denavit- 
Hartenberg parameters and the homogeneous 
transformation matrices relating successive coordi- 
nate frames are presented. The forward position 
transformation is factored for efficient computation. 

The inverse position solutions are presented next. 
Two angles are specified and the remaining six are 
solved. In this paper, one joint angle must be 
specified from the arm joints (1-3) and one from 
the wrist joints (5-8). Choosing two wrist joints 
is possible, but it leads to an underconstrained set 
of equations for the arm joints, and these equations 
are not dealt with in this paper. The elbow joint 
angle #4 is solved independently of the remaining 
joint angles and cannot be specified. The length of 
reach from the shoulder to the wrist determines the 
elbow joint angle #4 with two possible configurations, 
elbow up and elbow down. All twelve combinations 
of specified joints are allowed in the methods of this 
paper. For each combination, eight inverse position 
solutions exist. 

The velocity solutions follow the position solu- 
tions. The forward velocity solution is a linear trans- 
formation from joint rates to Cartesian rates through 
the Jacobian matrix. The 6x8 Jacobian matrix is 
presented with respect to the base frame and with 
respect to the elbow frame. The Jacobian matrix 
with respect to the elbow frame involves less sym- 
bolic terms than any other frame for the ARMII. 
The computation of either Jacobian matrix involves 
terms from the forward position transformation. 


position and velocity kinematics, the forward solu- 
tion output is the inverse solution input used to ver- 
ify the results. 

The methods used for derivation of the forward 
kinematic equations in this paper are existing stan- 
dard methods in robotics. A computer symbolic 
manipulation program was used extensively for 
derivation of the equations. In addition, computer 
simulations were developed to verify the equations. 
The inverse position solutions are original work based 
on an adaptation and extension of reference 11. The 
principal contribution of this paper is the first pre- 
sentation of efficient position and velocity kinematic 
equations for the ARMII. 


2. Symbols 


ARMII 

a i — 1 

Ci 

di 

^3,^5 

Jij 

J* 

K u KK % 
Li 


Advanced Research Manipulator II 
Denavit-Hartenberg parameter 
cos 

Denavit-Hartenberg parameter 

Denavit-Hartenberg parameters, 
fixed manipulator lengths 

element (i, j) of Jacobian matrix 

Moore-Penrose pseudoinversc of 
Jacobian matrix 

factored terms 

length from base to shoulder 


The resolved motion rate, or inverse velocity, 
problem (ref. 10) is solved in a manner similar to 
the inverse position problem. Two joint rates are 
specified, one from the arm joints (1-3) and one 
from the wrist joints (5-8). The elbow joint rate 64 
cannot be specified because it is uniquely determined 
by the Cartesian translational velocity command. 
The resolved motion rate problem is solved in closed 
form for the Jacobian matrix with respect to {4}. 
The inverse velocity solution is unique, provided that 
the Jacobian matrix has full rank. In this paper, 
singularity solutions are not presented; that is, the 
Jacobian matrix is assumed to have a rank of six. 

An identification of ARMII singularities is based 
on specifying two joint rates in the resolved motion 
rate problem and analyzing the reduced Jacobian 
matrix. Singularity conditions are presented for all 
specified joint combinations. No alternate singularity 
solutions are developed. 

Examples are presented to demonstrate the equa- 
tions for all solutions given in this paper. For both 


L 8 

length from wrist to end effector 

mj 

Jacobian matrix expressed in {m} 

m J. LL 

lower left partition of m J 

LR 

lower right partition of 777 J 

LRj 

m J lr with column j removed 

LRj} 

column j of m J/ y # 

m J UL 

upper left partition of m J 

m J UR 

upper right partition of m J 

m { J l ULi) 

column i of m J jjl with row 1 
removed 

m ^lULi4 

m ^UL with columns i and 4, plus 
row 1 removed 

\\P\\ 

Euclidean norm of vector P 

{ n P™} 

position vector from origin of {n} 
to {m}, expressed in {n} 

{P x ..Py,Pz) T 

components of {°Ps} 
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n d 

771 R 

orthonormal rotation matrix of 
{m} relative to {n} 

Rij 

element ( i,j ) of gR 

r ij 

element (z, j) of gR 

Sj 

sin Qj 

Tl >T> 

m A 

homogeneous transformation 
matrix of {m} relative to {n} 

u 

tan 9 t 

{”Vfc} 

linear velocity from origin of { k } 
with respect to {0}, expressed in 
{m} 

X/H) * 7 , 1 , Z , n 

unit direction vectors of {m} 

™{X} 

m {x,y,z,uj x ,ujy,u; z } T , 

{{ m v 8 },{ m u, 8 }} r 

a i- 1 

Denavit-Hartenbcrg parameter 

0 l 

joint angle i 

0} 

eight ARMII joint angles, arm, 
and wrist (1-8) 

0}a 

{61,82,83, 8±} T , four arm joint 
angles (14) 

{0}w 

{$ 5 , 9 $, 67, 0$} T , four wrist joint 
angles (5-8) 

k 

joint rate i 

{0} 

eight ARMII joint rates, arm, and 
wrist (1-8) 

0}a 

arm joint rates 

0 } Ai 4 

arm joint rates, excluding i and 4 

0}w 

wrist joint rates 

0} Wj 

wrist joint rates, excluding j 


angular velocity of {k} with 
respect to {0}, expressed in {m} 


Mathematical notation: 

{ } Cartesian coordinate frame 

{., « } T vector components 
Arm reference points: 

S shoulder 

E elbow 


W wrist 

Coordinate frames: 

B base 

H end effector 

m dextral 

0 base for simplified kinematic 

equations 

4 elbow 

8 end effector for simplified kine- 

matic equations 


3. Kinematic Simplification 

3.1. Base and End-Effector Coordinate 

Frames 

For telerobotic tasks, the position and orienta- 
tion of the end-effector coordinate frame { H } are 
controlled with respect to the base coordinate frame 
{£?}. The symbolic terms for the forward position 
transformation and Jacobian matrix require signifi- 
cantly less calculations when {8} is controlled with 
respect to {0}. The origin of {8} is located at the 
wrist point W and the origin of {0} is located at the 
shoulder point S. (See fig. 3.) This removes L\ and Lg 
from the basic kinematic equations. No loss of gener- 
ality is incurred because control of {H } with respect 
to {£} is transformed to control of {8} with respect 
to {0} through equations (1), (2), and (3). Given 
^T, gT is calculated by the following equation: 


where 



H rp — 1 

0 1 


H - 


B 

0 


T -1 gT ^T _1 


-1 0 0 0 - 
0 10 0 
0 0 1 -L\ 
.0 0 0 1 . 

-1 0 0 0 - 
0 10 0 
0 0 1 -l 8 
.0 0 0 1 . 


( 1 ) 


Given the Cartesian translational and rotational 
velocities {^v//} and the equivalent Carte- 

sian velocity command at {8} is calculated as 


{*W - { H u h } (2) 

{ 8 v 8 } = { // v H }-{ 8 u; 8 }x{ 8 P H } (3) 

where { 8 P//} = {0,0, L$} T . A velocity transforma- 
tion is not required between {£?} and {0} because no 
relative motion occurs. Equations (1), (2), and (3) 
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are written for the inverse position and velocity prob- 
lems. The same equations can be modified for use in 
the forward position and velocity problems. 

3.2. Decoupling Position From 

Orientation 

An efficient method for calculating kinematic so- 
lutions of manipulators with spherical wrist mecha- 
nisms is to decouple the position from the orienta- 
tion. The arm joint angles position W in space and 
the wrist joint angles orient {8} with respect to {4}. 
The wrist joint rotations do not affect the positioning 
of the arm joints. The ARMII has a four-axis spher- 
ical wrist. Decoupling the position from the orienta- 
tion applies to both position and velocity problems. 

From reference 9, the general form of gT is 


or 

{°p 8 } 

000 

1 


Equation (4) gives the decoupling of the position 
from the orientation as follows. Because the spheri- 
cal wrist causes no translations, the position vector 
{°Pg} is a function of only the arm joint angles. The 
manipulator orientation is provided by the wrist joint 
angles relative to the orientation of {4}, 

The terms for the forward position transforma- 
tion, equation (4), are presented in the next section. 
The position vector {°Ps} is expressed as a function 
of 62 , # 3 , and 0 4 . The rotation matrix represent- 

ing the manipulator orientation is given as a function 
of all joint angles. 

The Jacobian matrix used in velocity kinematics 
has the following form when the wrist is spherical: 


j = 

JUL 

1 

L_°_ 


J LL 

— 1 


The wrist joint rates do not affect the translational 
Cartesian velocity of the end effector; thus, the 
upper-right portion of the Jacobian matrix is the zero 
matrix. The Cartesian angular velocity is a function 
of all joint rates. 

4. Position Kinematics 

4.1. Forward Position Kinematics 

4.1.1. Denavit-Hartenberg parameters . Fig- 
ure 3 defines the coordinate frames for the ARMII. 



Figure 3. ARMII coordinate frames. {0} = {0, 0, 0, 0, 0, 0, 0, 0}. 

The manipulator pose in figure 3 is the initial po- 
sition where all joint angles are 0. The Xq, Xi, 
and X 2 axes are coincident in the initial position. 
The same is true of X 3 and X 4 and also X 7 and Xg. 
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The Z 2 , Z 4 , and Z 7 axes are directed outward, per- 
pendicular to the plane of the paper. For these co- 
ordinate frame definitions, the eight sets of Denavit- 
Hartenberg parameters relating the nine successive 
coordinate frames { 0 } through { 8 } are given in ta- 
ble 1. With the simplification presented in sec- 
tion 3.1, the parameters d\ and dg are both 0, In 
figure 3, the lengths from the base to the shoulder 
and the wrist to the end effector are labelled L\ and 
Lg to avoid confusion with the Denavit-Hartenberg 
parameters d\ and dg. The joint variables 9 Z are the 
angles from Xj_i to Xj measured about Z$. Joints 
five, six, and seven require the offsets given in table 1 
for # 5 , # 6 , and 0 j to be 0 in the initial configuration 
of figure 3. 


Substituting the matrices of appendix B into equa- 
tion (7) yields 


U rp 

£ A — 


Kn s 8 ~ K T C 8 

Kqs 8 “ ^V c 8 

K S S 8 ~ K XC$ 


K t $8 + Kn c 8 

Kv s 8 + k Q c 8 
Kx s 8 + K S C 8 


0 0 


K U 

K W 

Ky 

0 


~ d 8 c \ s 2 “ K A~ 
~d 3 sis 2 -d$K c 
<*3 C 2 “ 

(8) 


Common terms K x , reported in appendix C, are fac- 
tored out to reduce computation time. Based on de- 
coupling the position from the orientation, discussed 
in section 3.2, the forward position transformation is 
partitioned at {4} as follows: 


§T = Sm ,02, 03, 04)im , e 6 ,9 7 ,9 & ) (9) 


Table 1. Denavit-Hartenberg Parameters 


i 

&i— 1 


4 

0 i 

1 

0 

0 

0 

0 1 

2 

90° 

0 

0 

0 2 

3 

-90° 

0 

d 3 

#3 

4 

90° 

0 

0 

0 4 

5 

-90° 

0 


0 5 - 90° 

6 

-90° 

0 

0 

0 6 + 90° 

7 

90° 

0 

0 

0 7 - 90° 

8 

90° 

0 

0 

08 


Nominal values for the fixed lengths are d% = 
762.0 mm and d§ — 495.3 mm. The fixed length 
L\ depends on the manipulator mounting and Lg 
depends on the end effector. Nominal joint limits 
are given in appendix A. 

4*1*2. Homogeneous transformation ma- 
trices. The general homogeneous transformation 
matrix (ref. 9) represents the position and orienta- 
tion of {z} with respect to {i — 1} and is given as 
follows: 


i— 1 r 

i 


c6i —s9i 0 

sOiCOii-. 1 cOjCOLi — | 

s9iSCii-\ c9isai_i cct 2 ;_i 

0 0 0 


a i-l 
disoti—\ 
diSOt{-~ 1 
1 

( 6 ) 


The Denavit-Hartenberg parameters are substituted 
into equation (6) to produce eight homogeneous 
transformation matrices (given in appendix B) that 
relate successive coordinate frames. 


4*1*3. Forward position transformation. 
The forward position transformation is a unique 
mapping from joint space to Cartesian space: 

°T = ?T(0! ) £T(0 2 ) §T(0 3 ) • • -l T(0 8 ) (7) 


where 


\K B 

-K A 

K 2 

— d 3 CiS2 " 

Kd 

-K C 

K 4 

—d 3 s\S2 

Kh 

-K 6 

S2S3 

d 3 C2 

. 0 

0 

0 

1 


gT = 


55Cg58 - KK4C8 

KK b 

C5C658 + KK 2 c% 
0 


KK 4 S 8 -j- sscgcg K 0 
KKq c$c 7 

—KK 2 s% 4- C 5 C 6 C 8 KK] 0 

0 0 1 


The KK t terms are defined in appendix C. 

4 .I. 4 . Geometric arm joint redundancy. 
Figure 4 presents the geometric interpretation of 
the arm joint redundancy for the ARMII. The arm 



Figure 4. Geometric interpretation of ARMII arm joint 
redundancy. 
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redundancy or self-motion is the rotation of the elbow 
point E about {°Pg}. A given Cartesian position and 
orientation is reachable at any of these locations of E. 
The radius of this rotation r varies with {°Pg}. The 
wrist joint redundancy provides a freedom in addition 
to this self-motion behavior. 


The vector {°Pg} gives the position of the 
wrist W with respect to the shoulder S, as shown 
in figure 5. This length-of- reach constraint fixes the 
value of 84 . The plane of triangle SEW is perpen- 
dicular to Z 4 for all manipulator configurations. The 
law of cosines is used to solve 64 so that 


4.2. Inverse Position Kinematics 


||{°P 8 }|| 2 = 4 + ( ii -2(^5 cos (11) 


The inverse position problem is a mapping from 
Cartesian space to joint angle space. This problem 
is more complicated than the forward problem be- 
cause it involves coupled transcendental equations 
with multiple solutions. The inverse position prob- 
lem calculates 0i,02,...j08 ^ or ARMII, when 
given the following Cartesian position and orienta- 
tion command gT: 

-Rll R12 R43 Px 

°T = ^ 21 ^ 22 ^ 23 

8 P31 P32 P33 Pz 

. 0 0 0 1 

Equation ( 8 ) expresses [jT in terms of the unknown 
joint angles. Equations (10) and (8) are equated to 
obtain the inverse position equations. This equating 
yields twelve scalar equations, only six of which 
are independent. The six independent equations 
come from the three position terms and three of the 
possible nine rotation matrix terms. 

The inverse position problem cannot be solved 
for redundant manipulators without additional con- 
straints. The specified Cartesian location has six de- 
grees of freedom but eight one-degree-of- freedom 
joints. For the ARMII, the inverse position prob- 
lem is an underconstrained set of six equations in 
eight unknowns. In this section, the inverse position 
problem is solved by specifying two joint angles. 

Decoupling of the position from the orienta- 
tion is utilized in the inverse solutions. The arm 
joint angles are solved from the position command 
{°Pg}. The orientation command and the influ- 
ence of arm joint angles on the orientation are then 
used to solve for the wrist joint angles. Therefore, 
the inverse equations are two sets of three equa- 
tions in four unknowns. One joint angle is specified 
from the arm joints and one from the wrist joints. 
These methods are an adaptation and extension of 
reference 11. 

4*2.1. Inverse position solutions for arm 
joint angles . Multiple solutions for { 8 \, ® 4 } T 

are obtained in this section with {°P§} given. A 
geometric approach is used to solve for 64 first, 
independently of { 0 \ y #3} T and {^5, ^6? ^7^ ^8}"^* 



where || {°Ps } l| 2 — P\ + Py + Pf and <j> — tx — 64 . 
The two 84 solutions to equation (11) are as follows: 


}p 8>H 2 -djj ~dj 

2 d 3 d 5 




Figure 5 . Geometric method to calculate 84 . 


A geometric method is used to determine whether 
a given position command is within the manipulator 
workspace. The maximum reach occurs when d 3 and 
d§ align ( 64 =0). At the minimum reach, d§ folds 
back upon d% ( O 4 = 7r). Based on these conditions, 
the following inequalities must be satisfied for {°Pg} 
to be reachable: 

1*3 -dsl < l|{ 0 P 8 }ll <<*3 + <* 5 (13) 

This analysis ignores joint angle 84 limits, which 
cause a more restricted workspace. 

The remaining arm joint angles { 81 , 82 , 83 ^ are 
solved with algebra and trigonometry. The wrist 
mechanism is spherical; thus {°P5} equals {°Pg}. 
Equations for {0 i, 02>#3} T are obtained from the 
identity 

{ 0 P5}-5R(^1^2,%^4){ 4 P5} (14) 
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where {°P5} = {Px,Py>Pz} T is the position com- 
mand and { 4 P 5 } = {0, d 5 , 0} T is known. Equa- 
tion (14) is rewritten as 

2r(0 1 ,0 2 ){ o P 5 } = lR(03^4){ 4 P 5 } (15a) 

and expands to the following three scalar equations: 

{Px c 1 + Py s l ) c 2 + Pz s 2 = -^50354 (15b) 

-(Px c l + Pysi)s 2 + Pz £ 2 — ^3 + d$C 4 (15c) 
p X$ 1 - p Y c l = <^$354 (15d) 

Equations (15b), (15c), and (15d) can apparently 
be solved for the three unknowns (0i,02>03) because 
0 4 is known. However, squaring equations (15b), 
(15c), and (15d) and adding them gives the cosine 
law used previously to solve $4. Therefore, equa- 
tions (15b), (15c), and (15d) are two independent 
equations in three unknowns. One joint unknown is 
specified and the other two are solved. Three cases 
are presented that correspond to specified 0 1, #2 » 
or #3. For each case, 64 is known from equation (12). 

Case 1. With 9\ specified, equation (15c), re- 
written in the following equation, is solved for 62 as 
follows: 

E cos 62 + F sin 62 + G = 0 (16) 


where 

E = —P Z 
F = Pxc 1 + PyS\ 

G = d% -h d§C 4 

Equations of the form in equation (16) arise often in 
inverse position kinematics. The solution is obtained 
with the tangent half-angle substitution (ref. 13). 
Appendix D presents the two valid solutions for the 
general form of equation (16) with this method. 

A ratio of equations (15d) and (15b) is used to 
solve for #3. The quadrant-specific inverse tangent 
function is used to provide a unique result from the 
following equation: 


03 = tan 


P x s\ - Pyci 

~(PX C 1 + P Y S l) c 2 “ Pzs 2_ 


(17) 


Two 64 solutions are given in equation (12). For each 
0 4 , two 02 solutions are obtained from equation (16). 
Each 0 2 has one 0 3 solution (eq. (17)). Therefore, 
four solutions exist for the arm joint angles, with 
the joint limits ignored. The four solutions have the 
structure shown in table 2. 
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Table 2. Arm Joint Solutions With 9\ Specified 


n 

0 1 

02 

#3 

04 

1 

0i 

021 

03 

04 

2 

0i 

022 

~0 3 +7T 

04 

3 

0 1 

022 

-03 

~04 

4 


021 

03 + tt 

~04 


Case 2. With 0 2 specified, equation (15c) is solved 
to yield two values of 9 \ : 

E cos 0i + F sin 9\ + G = 0 (18) 


where 

E = P X S 2 
F = PyS 2 

G = c?3 + d$C 4 - P Z C 2 

The angle 03 is again calculated by equation (17). Ta- 
ble 3 gives the solution structure when 02 is specified. 


Table 3. Arm Joint Solutions With 02 Specified 


n 

0i 

02 

03 

04 

r j 

011 

02 

03 

04 

2 

012 

02 

-03 

04 

3 

011 

02 

03 +TT 

-04 

4 

012 

02 

-03 + 7T 

-04 


Case 3. With 03 specified, both 03 and 04 are 
known. Equations (15) are rewritten to separate the 
unknowns 0i and 02 as follows: 

Jr{°P 5 } = lR{ 4 P 5 ) (19a) 

Pxc 1 + F\'Si = -{d 5 C 3 s 4 )c 2 - (d 3 + d 5 c 4 )s 2 (19b) 
Pyc | Px& 1 ^5 S 3 S 4 (19c) 

Pz = (^3 + ^5 C 4) C 2 — (^5 C 3 S 4)S2 (19d) 

Equation (19c) is solved for two values of 0j, inde- 
pendently of 02, as follows: 

E cos 0i + F sin 0i + G = 0 (20) 


where 

E = Py 

F = ~P X 
G = ds53 5 4 

Equation (19d) is solved for two values of 02, inde- 
pendently of 0i, as follows: 

E cos 02 + F sin 02 + G = 0 (21) 


<»"' umwmm mini Nil RIIIHMI MkU.i III Mill I III in till II ilH ill iiiliiUlllililllillliNil Iflll lii|||| ||||| ill || III I mill I III Nil llliillilll 1 1 III lllli 111 111 ill III || III Mill ii lilillili I IMHI bT r ' 




where 

E = -(<*3 + ^5 C 4) 

F - d5C 3 s 4 

G = P Z 

Equations (19c) and (19d) are solved given each of 
the two 64 values from equation (12). There are eight 
possible solutions for sets of 6 \, 02 , and 64 , but only 
four are valid. For each value of 9 4 , equation (19b) 


is used to determine which 62 value corresponds to 
each 6 \. The solution structure is given in table 4. 


Table 4. Arm Joint Solutions With 63 Specified 


n 

0i 

02 

03 

04 

1 

011 

021 

03 

04 

2 

012 

0 22 

03 

04 

3 

012 + 7T 

-0 22 

03 

-04 

4 

011 +7T 

-021 

03 

-04 


4.2.2. Inverse position solutions for wrist joint angles. This section presents an algebraic method 
to solve the orientation part of the inverse position problem. Sets of { 0 §, 6 §, 67 , 6 %} T are solved given g R and 
{0i,02,03,0t } 7 • T 

The wrist joint angles orient {8} with respect to (4). The orientation of (4) depends on {0i, 02< 03i 04} • 
The wrist orientation command g R is calculated by the following equation: 


4 

8 


R = 




m 

H2 

ri3 

T2\ 

r 22 

r 23 

r 31 

r 32 

r 33 


(22) 


The elements of °R are given in equation (9b). The wrist inverse position equations are obtained by equating 
4 r from equation (9) and equation (22). The unknowns are separated as follows: 

Sr- 1 Sr = ( 23 ) 


The matrices for the left- and right-hand sides of equation (23) are 


— r 21 c 6 - ( r 31 c 5 + r ll s 5) s 6 
7*21 S6 - ( r 31 c 5 + m s 5) c 6 
m c 5 ~ r 31 s 5 


-C 22 C 6 - (r32C5 + r-i 2 s 5 )s 6 
5"22 s 6 - ( r 32 c 5 + r 12 s 5) c 6 
r l‘2 c 5 - r 32 s 5 


-^ 23^6 - (^ 33^5 + r 13 s 5) 5 6 
^23^6 - ( r 33 c 5 + r 13 s 5) c 6 
r 13 c 5 - r 33 s 5 


S7C8 -S7S8 -C 7 

— S8 — c 8 0 

-C7C8 C 7 s 8 -s 7 


Equation (23) contains nine scalar equations in four unknowns, three of which are independent. For solution, 
one unknown joint angle is specified and the remaining three are calculated. Four cases are presented for 
specified # 5 , 06 ■ O 7 , or 0 $- 

Case 1. With 63 specified, the joint angle 06 is solved from the (2,3) element of equation (23). The quadrant- 
specific inverse tangent function is not required because 6 q and 0g +7r are both valid solutions. The 6 q solution 


is 


0g — tan 1 


r 33 c 5 + r 13 s 5 
r 23 


(24) 


A ratio of equation (23) elements (3,3) and (1,3) yields Qj. The quadrant-specific inverse tangent function 
must be used because one valid 6 j value exists for each 0g. Similarly, 0s is solved from the ratio of the 
elements (2,1) and (2,2) of equation (23): 


07 - tan 


^33^5 - ri 3 C 5 

_ r 23 c 6 + ( r 33 c 5 + n 3 S5) s 6. 


(25) 
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9 g = tan 1 

Two wrist solutions exist for each {0i,02> #4} re- 

sult. The solution structure is demonstrated in 
table 5. 


( r 31 c 6 + r nS5)c6 ~ r 21^6 ' 
L ( r 32 c 5 + rio55)c6 - rooSc 


(26) 


The quadrant-specific inverse tangent function used 
to calculate one 9$ is valid for each 0 5 . The wrist 
angle 9$ is calculated by equation (26). One 9% exists 
for each #5. Table 7 gives the two wrist solutions with 
#7 specified. 


Table 5. Wrist Joint Solutions With $5 Specified 


n 

05 

06 

07 

08 

1 

05 

06 

07 

08 

2 

05 

06 + 7T 

-07 + TT 

08 + TT 


Case 2. With 6 g specified, the joint angle #5 
is solved from the (2,3) element of equation (23). 
The general solution for the following equation is 
presented in appendix D. Both $5 results are valid. 

E cos #5 + F sin #5 + G = 0 (27) 


where 

E ~ 7*33 Cfi 
F = r 13 c 6 
G = -r 23 S6 

The wrist angles 9 7 and 9g are calculated by equa- 
tions (25) and (26). One (#7, 0g) pair exists for 
each 05. The two wrist solutions are given in table 6. 


Table 6. Wrist Joint Solutions With 0g Specified 


n 

05 

06 

07 

08 

1 

051 

06 

07 

081 

2 

052 

06 

-0 7 

082 


Case 3. With $7 specified, the (3,3) element of 
equation (23) gives the following equation: 

E cos 05 + F sin #5 -f G = 0 (28) 


Table 7. Wrist Joint Solutions With 07 Specified 


n 

05 

06 

0 7 

08 

1 

051 

06 

07 

081 

2 

052 

-06 

07 

082 


Case 4. With 0g specified, two values for 05 are 
solved from the ratio of the (3,1) and (3,2) elements 
of equation (23) as follows: 


05 = tan 


>12 +m*8 ~ 

T32 + 


(30) 


Both 05 and 05 + tt are valid results. The unique 
06 is solved by equation (29). The wrist angle 07 is 
calculated by equation (25). For each 05 there is a 
unique 07. Table 8 gives the two wrist solutions with 
08 specified. 


Table 8. Wrist Joint Solutions With 9g Specified 


n 

05 

06 

0 7 

08 

1 

05 

06 

0 7 

08 

2 

05+71 

+ 

1 

07 + 7T 

08 


4-2.3. Overall inverse position kinematic 
solutions. The overall inverse position problem has 
eight solutions: four arm joint solutions and two 

wrist solutions for each. This is true for all specified 
arm and wrist joint angle combinations. The overall 
solution structure is greater than the individual arm 
joint and wrist joint tables indicate. Table 9 presents 
the eight solutions obtained with 9\ and 05 specified. 
Other combinations have a similar structure. 


where 

E = n 3 
F = -r 33 
C = s 7 

Appendix D presents a solution method for equa- 
tion (28). 05 has two valid solutions. The angle 06 is 
solved from the (2,3) element of equation (23) and is 


06 = tan 1 


^33 c 5 + H3 5 5 
r 23 


(29) 


Table 9. Overall Solutions With Q\ and #5 Specified 


n 

0i 

02 

03 

04 

05 

06 

07 

08 

1 

0i 

021 

03 

04 

05 

061 

071 

081 

2 

01 

021 

03 

04 

05 

061 + TT 

— 071 + TT 

081 + * 

3 

0i 

022 

-03 4- 7T 

04 

05 

062 

072 

082 

4 

0i 

022 

-03 + 7T 

04 

05 

062 + 7T 

— 072 + 7T 

082 4- TT 

5 

0i 

022 

-03 

-04 

05 

-062 

-072 

082 + TT 

6 

0i 

022 

-03 

-0 4 

05 

“062 -f TT 

072 + 7T 

082 

7 

0i 

021 

03 + ir 

-04 

05 

061 

-071 

081 + TT 

8 

0i 

021 

03 + TT 

— 04 


061 T 77 

071 + 7T 

081 
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5. Velocity Kinematics 

5.1. Forward Velocity Kinematics 

The forward velocity problem calculates the Cartesian velocities given the joint rates. The Jacobian matrix 
is a linear operator that maps joint space velocities to Cartesian velocities as follows: 

m {X}= m 3 { 9 } (31) 

In equation (31), m {X\ is the vector of Cartesian linear and angular velocities of {8} with respect to {0}, 
expressed in {m}. The dimension of {0} is eight for the ARMII. The Jacobian matrix order is 6 x 8. 

5.1.1. Jacobian matrix expressed in {0}. The Jacobian matrix form for m = 0 is as follows: 

Tli T12 «/i3 J14 0 0 0 

T21 T22 T23 T24 0 0 0 

Oj _ 0 J32 T33 T34 0 0 0 

0 T42 J43 T44 T45 T46 J47 

0 J52 T53 J54 J55 J56 T57 

. 1 0 J63 Jq 4 T55 Jqg T07 


0 1 

0 

0 

Tl8 

T58 

j 68 


(32) 


The upper-right Jacobian submatrix is the zero matrix because the spherical wrist joint rates 6*5 through 6g do 
not affect the translational Cartesian velocity. The first column of the Jacobian matrix shows that 6\ affects 
only °±, °y, and °u> z . The term J 6 i equals 1 because 9 \ adds directly to °w 2 in {0} coordinates. The term J 62 
equals 0 because 62 does not influence . Because of the decoupling of the position from the orientation, the 
form of the Jacobian matrix is that of equation (5). The Jacobian matrix terms are 


0 


j UL 


d$s\S2 + d$Kc 
-d$c\S2 - d 5 K A 
0 


-d 3 ciC2 + d 5 c\K 6 
— d 3 siC2 T d^s\K^ 
-Kz 


T5K2S4 -d^Kg 

d^K^SH - d;,l\ J) 

T5S2S3S4 -d^Ks 


°J 


LL 


•LR 


0 

51 

-Cl 5 2 

K 2 

0 

-ci 

-5152 

K 4 

1 

0 

C2 

S 2 S 3 


-K a 

-k h 

k n 

K V 

-K C 

-k l 

Kq 

K W 

-k g 

k f 

K S 

K y 


(33a) 


(33b) 


(33c) 


The terms Ki from the forward position transformation (eq. (8)) are given in appendix C. The Jacobian 
matrix °J is independent of 9 $. However, if velocities are transformed into {0} from velocities commanded 
in {8}, is involved. Of course, the end-effector Cartesian velocity depends on all joint rates, including 9 $. 


5 . 1 . 2 . Jacobian matrix expressed in {4}. 
The simplest symbolic form of the ARMII Jacobian 
matrix is presented in this section. This form is de- 
sirable because it reduces computation time for real- 
time manipulator operations. In addition, closed- 
form solutions to the inverse Jacobian submatrices 
are less complicated. 

A manipulator Jacobian matrix can be calculated 
by many methods. The vector cross-product method 
in reference 11 provides good physical insight into the 


problem. With this method, the simplest symbolic 
form of the Jacobian matrix results when it is based 
on the middle coordinate frame. When cross prod- 
ucts are taken from one end to the other (from {0} 
to {8} or vice versa), the terms compound greatly. 
Starting from the middle and working to both ends 
results in fewer Jacobian matrix terms. 

With given by equations (33), the Jacobian 
matrix referenced to any frame {m} is found and 
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Cartesian velocities expressed in { 0 } are transformed 
into {m} as follows: 


A = CI 3 C 4 + ^5 
B — d% + (I 5 C 4 



From properties of unitary orthogonal rotation 
matrices, 

tti -p 0 n -1 0 T} T 

0 it - — m ±l 

Substituting equation (31) for both m {X } and °{X} 
in equation (34) and using the preceding rotation 
matrix relationships yield the following equation: 



When equations (33) and (37) are compared, a great 
reduction in symbolic terms is evident. 

The terms K 1 and KKj from the forward posi- 
tion transformation are given in appendix C. The 
Jacobian matrix in {4} is independent of 9\ and 9 g. 
However, if velocities are transformed into {4} from 
velocities commanded in { 8 }, 6 s is involved; 6 \ is in- 
volved w r hen velocities are transformed into {4} from 
velocities commanded in { 0 }. 

5.1.3, Cartesian velocities expressed in { 8 }. 
The Jacobian matrix in {0} involves fewer symbolic 
terms than in { 8 }. In turn, 4 J is significantly sim- 
pler than °J. The symbolic form of 8 J is not re- 
ported. This section uses { 8 } as the coordinate frame 
to present the necessary transformations for velocity 
solutions. 


For the ARMII, {4} is the middle coordinate 
frame. The general form of 4 J is reported in equa- 
tion (36), obtained from m — 4 in equation (35): 

" J\\ J\2 0 J \ 4 0 0 0 0 - 

J21 J22 6 0 0 0 0 0 

4 j _ ^31 J32 J33 0 0 0 0 0 

J4I J42 ^43 0 0 J46 J47 J48 

Jhl ^52 ^53 0 1 0 J57 J58 

-<761 <762 0 1 0 J66 <767 *768- 

(36) 

Equation (9) is a symbolic representation of the ma- 
trix 4 R, and the 4 J terms are given in the following 
equation: 

<j= 

I 4 Jlr 

where 



The forward velocity problem using 777 J yields 
Cartesian rates expressed in {m}, where m — 0 or 4 
in this paper. Equation (34) is used when these rates 
are desired in { 8 } coordinates to give the following 
equations: 

{ 8 v 8 } = g l R T { 77 l vg} ) 

1 ( 38 ) 

{ 8 u, 8 }=^R r r W8 }J 

The input to the inverse velocity problem is m {X}. 
When these rates are expressed in { 8 }, equations (38) 
are inverted before m J is used in the inverse velocity 
solution as follows: 

rv 8 }=^R{ s v 8 } } 

„ (39) 

The rotation matrices ™ R are contained in equa- 
tion ( 8 ) for m = 0 and equation (9) for m = 4. 


— As 2«3 — Ac$ 0 — d .5 

4 JuL = d3 s 2 s 3 s 4 C?3C3S4 0 0 

_d3 s 2 c 3 + d^Ke, -R «3 d 5 s 4 0 

T -S3C4 «4 0 

4 J LL= ~ K & S 3 S 4 c 4 0 

S 2 S 3 c 3 0 1 

0 C 5 S 5 C 6 KK 3 
4 J LR =1 0 -S 6 cgc 7 

_0 — S 5 C 5 C 5 KK\_ 


5.2. Inverse Velocity Kinematics 

The inverse velocity problem solves the linear 
equation (31) for the joint rates when given a Carte- 
sian velocity command. Standard linear solution 
techniques cannot be used for a redundant manipu- 
lator because the Jacobian matrix is nonsquare. The 
inverse velocity problem for the ARMII is undercon- 
straincd with six equations in eight unknowns. Equa- 
tion (31) can be inverted with the well-known gener- 
alized inverse (ref. 13) of the Jacobian matrix. This 
redundant solution minimizes the Euclidean norm of 
the joint rates. 
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General redundancy resolution techniques are not 
presented in this paper. Instead, two joint rates are 
specified to solve the inverse velocity problem. The 
remaining system is six equations in six unknowns. 
A unique solution exists, when the manipulator is in 
a nonsingular configuration, i.e., when the Jacobian 
matrix has full rank. A square set of linear equations 
results only when one joint rate is specified from 
the arm joints and one from the wrist joints. This 
behavior agrees with the inverse position behavior. 
Specifying two wrist rates is possible, but it leads 
to an underconstrained set of equations for the arm 
joint rates, and these equations are beyond the scope 
of this paper. Additionally, joint rate 84 cannot be 
specified independently of the translational velocity 
command because of the structure of the ARMII. 
The length of reach from the shoulder to the wrist 
determines the elbow joint angle 64. A derivative of 
this constraint dictates that the elbow joint rate 64 
is uniquely determined by the Cartesian translational 
velocity command. 

5 . 2 . 1 . Independent solution for 64. The 
joint rate 64 is solved independently of the remaining 
seven unknown joint rates from a time derivative of 
equation (12). Equation ( 11 ) is rewritten as 

Px + Py + P% = d 3 T + 2 ^ 3^504 (40) 

Simplifying the time derivative of equation (40) yields 
the following solution for 8 4 : 

_ _ (p x G x + Py°y + P z °z ) 
dsd^s4 

In equation (41a), the Cartesian velocity command 
is expressed in { 0 }. When the frame of expression 
is {4}, 64 is simplified as shown in the following 
equation: 


where A is defined following equation (37). 

5 . 2 . 2 . Inverse velocity solution for remain- 
ing joint rates. The inverse velocity problem ex- 
ploits decoupling of the position from the orientation. 
Equation (31) is rewritten as 


fir + 


d^S4 


(41b) 



m Jf/L 

1 

o 

m *LL 

m y R \ 


{0 }a 

{ 


{ m v 8 } | 


(42) 


counting for the arm joint rates, the bottom three 
equations are then used to find the unknown wrist 
joint rates. The joint rate 84 is known from equa- 
tions (41). Therefore, column four of m IjjL 18 sub- 
tracted from the right-hand side of equation (42), 
multiplying by 64 . The remaining system is three 
equations in three unknowns. However, a unique 
solution to this system does not exist because it is 
always singular. The first two rows are dependent; 
the rank is two and not three. Either row one or two 
must be removed from the upper system of equations. 
The remaining system is two equations in three un- 
knowns, as for the arm angles in the inverse posi- 
tion solution. In this paper, the solution is achieved 
by specifying one joint rate from 8 \ } 62 , and #3 and 
then solving for the other two. The wrist joint rates 
are solved with the three equations in four unknowns 
from the bottom of equation (42), after the arm joint 
rates are obtained. One joint rate from $ 5 , # 5 , $ 7 , 
and 08 is specified and the remaining three are solved 
from the full-rank system, provided that the wrist is 
not in a singular configuration. 

Solution in frame {0} is obtained as follows. If 

from the arm joints and 8 j from the wrist are 
specified, columns i and j are removed from J[/L 4 
and J lr. Joint rate 8 { is likewise removed from 
{ 0 }^ 44 , and 6 j is removed from {0}w- In addition, 
to achieve a consistent set of equations for the arm 
joint unknowns, row 1 of J(jl 4 is removed; row 1 
of the Jacobian matrix in { 0 } or {4} is symbolically 
more complex than row 2. For m = 0, the solution 
is obtained with any linear solution method used for 
the following equations: 

°Jlt/Li 4 { 0 }xi 4 = {° v 8 } ~ ®{J\ULi) “ #4 ®{J\ULa} (43a) 

°JLRj{d}wj = {“<«*} - 0j - °Jll{Q}a (43b) 

The order of equation (43a) is 2 x 2. Row 1 of {°vg} 
is removed because the first equation of equation (42) 
is removed. The order of equation (43b) is 3 x 3 
because the wrist equations are of full rank for the 
general case. 

Solution in frame {4} is obtained as follows. The 
symbolic form of 4 J is simpler than °J, as demon- 
strated in section 5.1.2. When {4} is used as the 
reference frame, the linear equations are solved in 
closed form. The resolved motion rate solution for 
this case is given by the following equations: 

W AiA = 4j U7Li4 ({ 4 y 8 } ” 0* A {J\ULi) ~ ^4 4 {J\ULa}) (44a) 
Who = ({ 4 «s} - 4 { W - a 3u.$)a) (44b) 


The upper three equations of equation (42) are 
solved to yield the unknown arm joint rates. Ac- 
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Row 1 is removed from { 4 vg} in equation (44a), 
The symbolic terms for the inverse reduced Jacobian 
submatrices arc given in appendix E, for i = 1,2,3 
and j — 1,2, 3, 4. 

For either inverse velocity solution (eqs. (43) or 
(44)), the commanded Cartesian velocities must be 
transformed into {0} or {4} coordinates, unless they 
are specified in these frames. When Cartesian ve- 
locities are expressed in {8}, this transformation is 
accomplished with equations (39). 

6. Manipulator Singularities 

At a singular position, a manipulator loses one or 
more degrees of freedom. A near singular configura- 
tion mathematically requires infinite joint rates for 
certain finite Cartesian velocity commands. Singu- 
larity configurations for nonredundant manipulators 
are determined by equating the Jacobian matrix de- 
terminant to 0. For redundant manipulators, the Ja- 
cobian matrix is nonsquare and thus its determinant 
does not exist. 

The least-squares redundant solution to the in- 
verse velocity, or resolved motion rate problem, is 
obtained by inverting equation (31). 


{0} = m J* m {X } 

(45) 

In equation (45), 


J* = J T (JJ T )- 1 

(46) 


is the well-known pscudoin verse, or Moore-Penrose 
inverse of the Jacobian matrix (ref. 5). The singu- 
larities for a redundant manipulator can be found by 
setting the determinant of (JJ 7 ^) equal to 0, as evi- 
dent in equation (46). General redundant solutions 
and singularities, however, arc beyond the scope of 
this paper. 

The singularities reported in this section corre- 
spond to the inverse velocity solutions presented in 
section 5.2. Singularities are divided into arm singu- 
larities and wrist singularities for manipulators with 
spherical wrists. For the ARMII, arm singularities 
are identified from and wrist singularities 

from The order of the reduced Jacobian 

submatriccs used in section 5.2 is 2 x 3 and 3x4. 
When column 4 is removed from m J ui the determi- 
nant is 0 for any manipulator configuration. Thus, 
joint angle #4 and joint rate 64 cannot be specified 
in the inverse position and velocity solutions. From 
equation (41a) or (41b), the joint rate 64 is infinite 
when 64 — 0, 7r. This characteristic is a singularity 
condition for all arm joints, as shown in table 10. 


Table 10. Arm Joint Singularities 




Singularity 


i 

nuu\ 

conditions 


1 


*3 = ±f 

: 



64 = 0, 7T 

3 

2 

d^d^s^sj 

6 2 = 0,7T 




% = 0,7T 

- 



64 = 0, 7T 

— 

3 

-d^s^Kz 

64 = 0, 7T 

* 



K Z = 0 

1 


To find the arm joint singularity conditions, 
columns i = 1, 2, and 3 are removed individually 
from m J\uLiA and the remaining 2x2 determinants 
are set to 0. Similarly, columns j — 1, 2, 3, and 4 
are removed from m 3 LRj\ the 3x3 determinants are 
equated to 0 to yield the wrist joint singularities. 

Given a specific Jacobian matrix, such as °J, the 
Jacobian matrix referenced to any other frame m is 
found with equation (35). The singularity conditions 
are identical for Jacobian submatrices expressed in 
any coordinate frame because the determinant of a 
matrix is invariant under rotation transformations. 
In this paper, °J and 4 J are presented (eqs. (33) 
and (37)). The submatrices of either Jacobian matrix 
yield the arm joint singularities in table 10 and the 
wrist joint singularities in table 11. The condition 
Kz = 0 under i — 3 in table 10 is equivalent to 

d 3 = -t/5 (c± -F 


Table 11. Wrist Joint Singularities 


3 


Singularity 

conditions 

1 

-c 7 

-H 

II 

r~ 

QS 

2 

-c 6^7 

«6 = ±f 

0 7 = 0, 7T 

3 

— S6C7 

#6 = 0,7T 

0 7 = ±f 

4 

-c 6 

0 6 = ±f 


The results of tables 10 and 11 are also singular- 
ities for the inverse position solutions presented in 
section 4.2. This section has identified the ARMII 
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singularities associated with specifying one arm joint 
rate (excluding the elbow) and one wrist joint rate 
and solving the inverse velocity problem. Alternate 
solutions for the neighborhood of singularities are not 
presented. An alternative is to use the generalized in- 
verse (ref. 5) of the reduced 6x6 Jacobian matrix 
at or near singularities. At a singularity, the rank of 
this matrix is less than 6. The determinant is cal- 
culated at each calculation step. If it is near 0, the 
generalized inverse of the reduced Jacobian matrix 
is used to avoid infinite joint rates. This singularity 
solution does not track the given velocity command 
precisely, but it does move the manipulator out of 
the singularity region so that the solution given in 
this paper can be used again. 

7. Examples 

Examples are presented in this section for forward 
position, inverse position, forward velocity, and in- 
verse velocity problems to demonstrate the equations 
in this paper. The units are millimeters, degrees, 
millimeters per second, and radians per second for 
length, angle, translational velocity, and rotational 
velocity, respectively. The fixed manipulator lengths 
for the examples are as follows: 

Li = 500.0 
d 3 = 762.0 
d 5 = 495.3 
L 8 - 470.0 

7.1. Position Kinematic Examples 

7.1.1 . Forward position transformation. 
Two examples for the forward position transforma- 
tion are given. The first is the initial position shown 
in figure 3 and the second is a general configuration. 
For each example, the partitioned solution (eq. (9a)), 
the transformation from wrist to shoulder (eq. (8)), 
and the transformation from end effector to base 
(modified eq. (1)) are given. 


" -1.000 0.000 0.000 o.ooo - 
0 T= 0.000 - 1.000 0.000 0.000 

8 0.000 0.000 1.000 1257.300 

- 0 0 0 1 


- - 1.000 0.000 0.000 0.000 ■ 
B t= 0.000 - 1.000 0.000 0.000 

H 0.000 0.000 1.000 2227.300 

. 0 0 0 1 

Example 2 . {8} = {10.0,20.0, 30.0, 40.0,50.0, 60.0, -70.0, 80.0} t 

■0.331 -0.717 0.613 -256.660 ' 

0 T _ 0.447 -0.453 -0.771 -45.256 

4 0.831 0.529 0.171 716.046 

.0 0 0 1 


- 0.447 -0.331 0.831 0 ' 

4 T _ -0.771 -0.613 0.171 495.3 

8 0.453 -0.717 -0.529 0 

. 0 0 0 1 . 


-0.979 -0.110 -0.172 -611.971' 
0 T= 0.200 0.683 0.703 -269.549 

8 0.041 -0.722 0.690 978.284 

. 0 0 0 1 


-0.979 -0.110 -0.172 -692.958' 

b t= 0.200 0.683 0.703 60.660 

H 0.041 -0.722 0.690 1802.788 

.0 0 0 1 

7.1.2. Inverse position kinematics . The in- 
put for this example is gT from example 2 in the 
previous section. Eight solutions are calculated from 
the equations in section 4.2. The angles 9\ = 10 and 
0q = 60 are specified. Equations (12), (16), (17), 
(24), (25), and (27) are used for the results of ta- 
ble 12. The methods of sections 4.2.1 and 4.2.2 are 
used to form the multiple solutions. 


Example 1. {<9} — {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0} T Table 12 Inverse Position Kinematic Solutions 

- 1.000 0.000 0.000 0.000 - 
0 T= 0.000 0.000 - 1.000 0.000 

4 0.000 1.000 0.000 762.000 

. 0 0 0 1 . 

- -1.000 0.000 0.000 0 ■ 

4 T= 0.000 0.000 1.000 495.3 

8 0.000 1.000 0.000 0 

. 0 0 0 1 . 


n 0 \ 62 #3 

04 

05 

#6 

07 

08 

1 10 20.00 30.00 

40.00 

50.00 

60 

-70.00 

80.00 

2 10 20.00 30.00 

40.00 

-164.99 

60 

70.00 

23.04 

3 10 47.16 150.00 

40.00 

341.26 

60 

-33.24 

27.31 

4 10 47.16 150.00 

40.00 

-304.51 

60 

33.24 

-7.81 

5 10 47.16 -30.00 

-40.00 

161.26 

60 

-33.24 

27.31 

■6 10 47.16 -30.00 

-40.00 

-124.51 

60 

33.24 

-7.81 

7 10 20.00 210.00 

-40.00 

230.00 

60 

-70.00 

80.00 

8 10 20.00 210.00 

-40.00 

-344.99 

60 

70.00 

23.04 
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7.2. Velocity Kinematic Examples 

The manipulator configuration for the velocity examples is the input to forward position transformation 
(example 2): 

{0} = {10.0, 20.0, 30.0, 40.0, 50.0, 60.0, -70.0, 80.0} r 

7.2.1. Forward velocity kinematics. Given {0}, °J is calculated with equations (33), and given {0}, 
the forward velocity solution is calculated with equation (31): 

0 0 0 0 ’ 

0 0 0 0 

0 0 0 0 

-0.717 -0.257 0.945 -0.172 

-0.453 0.878 0.316 0.703 

0.529 0.403 -0.085 0.690 . 

{0} = {i.o^.o.s.o^.o.s.o^.o^.o.s.o} 77 

f -1727.3 ] f 1.90 ] 

{%} = < —2574.5 } {%} - { 5.60 \ 

{ -2781.8 J [ 14.50 J 

The Jacobian matrix relative to {4} is calculated with equation (37). The forward velocity solution is 
I calculated with i = 4 in equation (31), given the same {9} used previously. The resulting Cartesian velocities 

still relate {8} to {0} but are expressed in {4}: 

0 O' 

0 0 

0 0 

0.383 0.831 

-0.866 0.171 

0.321 -0.529. 

f -4034.6 ] f 15.18 ) 

{ 4 v 8 } = { 932.1 \ 3.78 

{ 451.0 J [ -0.68 J 

With ‘{R from the forward position transformation (example 2), 4 {X} transforms to the previous °{X} results 
and thus proves to be a consistent solution. The solution expressed in {8} is calculated with equation (38): 

f -2319.3 ] ( 3.57 ) 

{ 8 vg} = { 440.2 \ { 8 o> 8 } = { -6.85 J 

[ -3431.8 J [ 13.62 J 


7.2.2. Inverse velocity kinematics. Given {0} and the forward velocity results expressed in (0), the 
joint rates are calculated with equations (43). In this example, 02 — 2 and 05 — 5 are specified. Equation (41a) 
results in 04 = 4.00. The terms for equation (43a) are 

o _ [-611.971 -245.555] 

J 1 UL 24 — [ o 54 445 J 

Or t l _ / —169.877 1 

| _ 6 49 . 4 80 / 


-- 184.524 -934.464 0 -495.300 0 0 

83.761 424.183 0 0 0 0 

4 637.259 -570.711 318.373 0 0 0 

J “ 0.831 -0.383 0.643 0 0 0.643 

0.529 0.321 0.766 0 1 0 

. 0.171 0.866 0 1 0 -0.766 


-963.422 

-169.877 

-649.480 

0.174 

-0.985 

0 


195.192 

-245.555 

54.445 

-0.337 

-0.059 

0.940 
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f -221.538 1 


'0.831 

-0.383 

0.643 

O' 

\ -411.556 / 

II 

0.529 

0.321 

0.766 

0 

1 3 O * 


0.171 

0.866 

0 

1 


The solution for joint rates 5,6, and 7 is 


W.424 = 



The terms for equation (43b) are 


{®}w4 = 


5.00 1 

6.00 } 
7.00 J 


°J LRl 


-0.257 

0.878 

0.403 


0.945 

0.316 

-0.085 


-0.172 

0.703 

0.690 


°J LL 



( 

’ -0.7171 



-0.453 



l 

0.529 J 

1 

0 

0.174 

-0.337 

0.613 

0 

-0.985 

-0.059 

-0.771 

i 

0 

0.940 

0.171 


The solution for joint rates 6, 7, and 8 is 


f 6 00 ) 

Wwi = { 7.00 > 

[ 8.00 J 


The same inverse velocity problem is solved in 
closed form with equations (44), with respect to the 
elbow coordinate frame {4}. The input is the forward 
velocity results expressed in {4}; 6% = 3 and — 8 
are specified. Equation (41b) yields #4 = 4.00. The 
terms for equation (44a) are 

0.0018 0.0013 ' 

0.0020 —0.0003 


4j-l __ 
J IULM — 


4 {Jit/L3> 1 318.373 

4 {JlC/L4} = ( 0 } 


The solution for joint rates 1 and 2 is 


w.434 = 



The terms for equation (44b) are 


4j-l _ 
J LR4 ~ 


1.3268 1 

0.6428 0 
1.5321 0 


1.1133 

-0.7660 

1.2858 


( 0.831 1 
4 M= 0.171 } 

[ -0.529 J 


8. Concluding Remarks 

This paper presents the forward position kine- 
matics (given the eight joint angles, how to find 
the Cartesian position and orientation of the end ef- 
fector) and forward velocity kinematics (given the 
eight joint rates, how to find the Cartesian transla- 
tional and rotational velocities of the end effector) 
for the redundant eight-degree-of- freedom Advanced 
Reseach Manipulator II (ARMII). 

Inverse kinematic solutions, required to control 
the manipulator end effector, are also presented. For 
a redundant manipulator, the inverse kinematic solu- 
tions are not unique because they involve solving for 
eight unknowns (joint angles for inverse position and 
joint rates for inverse velocity) in only six equations. 
The approach in this paper is to specify two of the 
unknowns and solve for the remaining six unknowns. 
Two unknowns can be specified with two restrictions. 
First, the elbow joint angle and rate cannot be spec- 
ified. The elbow joint angle is determined solely by 
the commanded end-effector position. Likewise, the 
elbow joint rate is determined by the commanded 
end-effector Cartesian translational velocity. Second, 
one unknown must be specified from the four-jointed 
wrist, while the second unknown must be specified 
from one of the arm joints (elbow joint excluded) 
that translate the wrist. 

In the inverse position solution, each set of two 
specified joint angles has eight sets of solutions. No 
alternate inverse position solutions are presented for 
singular configurations. In the inverse velocity prob- 
lem, with two specified joint rates, the solution is 
unique, provided that the Jacobian matrix is not 
singular. A discussion of singularities is based on 
specifying two joint rates and analyzing the reduced 
Jacobian matrix. When the reduced Jacobian ma- 
trix is singular, the generalized inverse can be used 
to move the manipulator away from the singularity 
neighborhood. 

With two redundant joints, the methods of this 
paper allow considerable freedom in solving the in- 
verse kinematic problems. Either joint angles or rates 
must be specified for one of the three arm joints 
and one of the four wrist joints at each calculation 
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step. Control strategies will be developed as actual 
ARMII hardware experience is accumulated. A sim- 
ple method for control would be to lock two joints 
for all motion, for example, joints three and five or 
joints three and six. To accomplish this method, the 
locked joint angles and rates would be specified as 0 
for all motion. However, the methods of this paper 
allow more flexibility. 

A computer symbolic manipulation program was 
used with existing standard methods in robotics for 


the derivation of the equations. In addition, com- 
puter simulations were developed to verify the equa- 
tions. Examples demonstrate agreement between for- 
ward and inverse solutions. Research into applied 
redundant control strategies is required to realize the 
potential of the ARMII. 


NASA Langley Research Center 
Hampton, VA 23681-0001 
June 3, 1992 
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Appendix A 

ARMII Nominal Joint Limits 

The nominal joint limits for the ARMII are given in table Al. The wrist pitch angle, i = 7 in table Al, 
is severely limited in the positive direction. The mist roll is continuous and unlimited in both directions, as 
shown for i = 8 in table Al. 


Table AL ARMII Joint Limits 


i 

Oi 

1 

±165° 

2 

±105° 

3 

±165° 

4 

±105° 

5 

±165° 

6 

±165° 

7 

+22°, -130° 

8 

Continuous, 

bidirectional 
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Appendix B 

Homogeneous Transformation Matrices 

Eight homogeneous transformation matrices are given in this appendix, and they relate frame {;'} to 
{i — 1} for the ARMII, where i = 1,2, Substituting the Denavit-Hartenberg parameters of table 1 

into equation (6) yields these matrices: 



'Cl 

“51 

0 

0- 


?T — 

51 

Cl 

0 

0 


1 A — 

0 

0 

1 

0 



. 0 

0 

0 

1. 



" c 2 

“52 

0 

0- 

II 

0 

52 

0 

C2 

-1 

0 

0 

0 


. 0 

0 

0 

1. 


c 3 

-53 

0 


0 

2rp 

3 1 - 

0 

“5 3 

0 

-C 3 

1 

0 

d 3 
0 


. 0 

0 

0 


1 


■ C4 

-54 

0 

0- 

4T = 

0 

54 

0 

C 4 

-1 

0 

0 

0 


L 0 

0 

0 

1. 


■55 

C5 

0 

0 

-1 

|t = 

0 

c 5 

0 

-55 

1 

0 

^5 

0 



. 0 

0 

0 

1 

- 


"“56 

“ c 6 

0 

0- 

|t = 

0 

“C 6 

0 

56 

1 

0 

0 

0 


. 0 

0 

0 

1. 


" 57 

c 7 

0 

O' 

fT = 

0 

- c 7 

0 

57 

-1 

0 

0 

0 


. 0 

0 

0 

1 

J 

1 

” c 8 

5g 

0 

0- 

00 KI 

H 

II 

0 

58 

0 

c 8 

-1 

0 

0 

0 

1 

. 0 

0 

0 

1 

J 
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Appendix C 

Factored Kinematic Terms 

This appendix presents the kinematic terms factored for efficient computation of the forward position 
transformation matrices and the Jacobian matrices in { 0 } and { 4 }. The common terms for equation (8) and 
equations ( 33 ) and ( 37 ) are as follows: 


K\ = -51S3 + C1C2C3 

K2 = S1C3 + C1C2S3 

K 3 = C1S3 + S1C2C3 

K4 — -C1C3 + S1C2S3 

K z — C2S4 + S2C3C4 

Kq = -C2C4 + S2C3S4 

Kj = S 2 c 4 + C2C3S4 

K A = K\S4 + C1S2C4 

Kg = K\C4 — C1S2S4 

Kq = K Z S A + S1S2C4 

Kg = K z c a - S1S2S4 

Kg = K5S5 + S 2 S 3 C 5 

Kp = K z c z — S 2 S 3 S 5 

Kq — KgS Z + K 2 C Z 

Kg = —KgC Z + K 2 S Z 

Kj = Kps z + -K4C5 

Kp = —Kp)C§ + K4S5 

Km — Kg s 6 “ K a cq 

K N = Kqc§ + K A s 6 

Kp = Kjsq - Keep 

Kq = Kjce + KqSq 

Kp = K e sq — Kqcq 

Ks = Kpcp + A'fi.Sfi 

Kp = K m s 7 - K h cj 

K{j = K m ct + K h sj 

Ky — KpSf — KpCy 

K\y = KpCf + K jS~ 

Kx = Kr s 7 + Kpc-j 

Ky — Kr c 7 - Kf s 7 

Kz = d z s 2 + d z Kj 


The terms for equation ( 9 ) and equation ( 37 ) are as follows: 

KK\ = s 5 s 7 + c 5 s 6 c7 
KK 2 = S5C7 - c5.sfi.s7 

KK Z = — C5S7 + S5S6C7 
KK a = C5C7 + S5S6S7 
KK Z = -S6S 8 - cfis 7 c 8 
KKq = -s$cg + C6S7S8 


21 



Appendix D 

Solution of E cos [3 + F sin (3 + G = 0 

The general solution to the following equation is presented in this appendix: 


Ecos/3 -f F sin j3 -f G — 0 (Dl) 

In equation (Dl), E, F\ and G are constants and (3 is unknown. The tangent half- angle substitution is used 
to transform equation (Dl) from a transcendental to a polynomial expression: 


t = tan — 
2 

(D2) 

1 - t 2 

cos/3=i + t 2 

(D3) 

2 1 

sm/?=i +t2 

(D4) 


Substituting equations (D3) and (D4) into equation (Dl) yields the following polynomial equation: 

(G - E)t 2 + 2Ft + {G + E) = 0 (D5) 


The equation has two solutions: 


* 1,2 = 


—F ± \/£ 2 + F 2 - G 2 
G - E 


(D6) 


The first-order transcendental equation (eq. (Dl)) has been transformed into a second-order polynomial 
equation (eq. (D5)). The two corresponding values of (3 are found by inverting equation (D2) and substituting 
equation (D6). Both results are valid solutions for equation (Dl): 


,2 — 2 tan 1 


— F ± V E 2 + F 2 - G 1 
G-E 


(D7) 


Because of the multiplying factor of 2 in equation (D7), the quadrant-specific inverse tangent function is not 
required. The two-quadrant inverse tangent function suffices, unless G equals E, 
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Appendix E 

Inverse Jacobian Submatrices 

The symbolic form of 4 J requires the least computation for any ARMII 777 J matrices, as demonstrated in 
section 5.1.2. One advantage of 4 J is the ability to apply closed-form solutions for the resolved motion rate, 
or inverse velocity, problem in real-time computation. This appendix presents the inverses of the reduced 
Jacobian submatrices 4 Jh7L?:4j i = 1,2,3, and 4 J lrj, j — 1,2,3, 4, for use in equations (44). 

When the joint rate is specified for the first, second, or third arm joint, the following inverse matrices are 
used. The order of the matrices in equations (El) through (E3) is 2 x 2 because the elbow joint rate # 4 is solved 
(see eq. (41b)) independently of the remaining joint rates. Two of the three translational velocity equations 
are independent; i = 1, 2, or 3 is specified and the ot her two arm joint rates are solved. The first subscript 1 
in the following equations indicates that row 1 was eliminated from equation (42): 


4j-l 
J 11/144 


33C354 ° 

BU 1 

d 3 d 5 s 4 ^5 5 4 


4j-i 
J WL2A 


^3 5 2 5 3 s 4 
dzS2C3 + dr^Ks 
d3d5S2.s3.S4 


0 

1 

S554 


4j-l 
J IU L34 


Eg, 

^3 S 2 C 3 + 

D 


h 

55? 


Z J 


(El) 


(E2) 


(E3) 


where 

D = d 3 5 4 (Ss2 4- rf5 c 2^3^4) 

The term B is defined from 4 J (eq. (37)): 

B = f/ 3 + (IrjC.i 

The terms K§ and K% are defined in appendix C. 

The following inverse matrices are used when the joint rate is specified for the fifth, sixth, seventh, or eighth 
manipulator joint (corresponding to j = 1, 2, 3, 4): 


4 J 


-1 

LRl 


4 J 


-1 

LR2 


' KKi 

c 6 S7 -KKi~\ 

S5C6C7 

- 

-■%('! C5C6C7 

^ 5^6 


C6 C 5 S 6 

KK { 

1 

-KK 21 


c 6 

C 6 


KK l 

0 

-KK j 


^6 

c 6 


- -C 5 

0 

s 5 - 



(E4) 


(E5) 
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(E6) 



The order of these matrices is 3 x 3 because the three rotational velocity equations are independent . The terms 
KKi are defined in appendix C. 
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